package simunrobot.core;

import java.util.Vector;

import processing.core.PMatrix3D;

import remixlab.proscene.InteractiveFrame;
import simunrobot.io.geoObjectMini;

enum Modes {DENAVIT_HARTENBERG,SCREW};
enum Item { ROBOT, ESLABON, DUMMY };

public class SItem {
	static float PI = 3.14159f;
	static float PI_OVER_2 = 1.57079632679489661923f;
    
    /** \brief Clase Base para los Items.
    		Clase Base para los Items de un Robot
    	Es la Clase de la que heredan todos los objetos que quieran ser representados en el mundo 3D
     */

		///! Constructor
        public SItem(SItem parent)
        {
        	parentItem = parent;
            aframe = new InteractiveFrame();
            Geometria = new geoObjectMini();
            _id = 0;
            mode=Modes.DENAVIT_HARTENBERG;
            amin=-1000;dmin=-1000;thetamin=-1000;alphamin=-1000;
            amax=1000;dmax=1000;thetamax=1000;alphamax=1000;
            changedbywidget=false;
            childItems = new Vector<SItem>();
            OriginalParameters = new double[4];
            denavit = new double[4];
        }
        
		/**
		 * Retorna el item padre
		 	\return SceneItem, padre
		 */
        public SItem parent(){return parentItem;}
        
        /**
		 * Retorna el item hijo
		 	\return SceneItem, hijo.
		 */
        public SItem child(int number){return childItems.elementAt(number);}
        
        /**
		 * Retorna el la cantidad de hijos que contiene el Item
		 	\return int numero de hijos
		 */
        public int childCount() {return childItems.size();}
        
        /**
		 * Retorna la posicion de este item en la lista del padre
		 	\return int numero de hijos
		 */
        public int childNumber() {
        	if (parentItem!=null)
        		return parentItem.childItems.indexOf(this);
            return 0;
            }
        
        public boolean insertChildren(int position, int count, int columns){
            return false;
        }
        public boolean insertChildren(SItem item){
                childItems.add(item);
            return true;
        }
        public boolean removeChildren(int position, int count){
        	if (position < 0 || position + count > childItems.size())
                return false;
        	for (int row = 0; row < count; ++row)
        		childItems.removeElementAt(position);

            return true;
        }
        
        public boolean setData(int column, String value){return false;}

        public InteractiveFrame getFrame(){return aframe;}

        public void draw(){
            Geometria.draw();
        }
        public void setGeometria(String filename){
        	Geometria.askfile(filename);
        }
        public void setDenavit(String which,double value){
        	if(which == "a")
                denavit[0]=value;
            if(which == "d")
                denavit[1]=value;
            if(which == "theta")
                denavit[2]=value;
            if(which == "alpha")
                denavit[3]=value;
        }
       
        public void setMode(simunrobot.core.Modes Mode){mode=Mode;}
        public PMatrix3D getMatrix(SItem parent) {
        	PMatrix3D matrix =  new PMatrix3D();
        	matrix.m00=(float)Math.cos(parent.denavit[2]);                              	matrix.m10=(float)-Math.sin(parent.denavit[2]);                             	matrix.m20=0;
            matrix.m01=(float)(Math.sin(parent.denavit[2])*Math.cos(parent.denavit[3]));	matrix.m11=(float)(Math.cos(parent.denavit[2])*Math.cos(parent.denavit[3]));	matrix.m21=(float)-Math.sin(parent.denavit[3]);
            matrix.m02=(float)(Math.sin(parent.denavit[2])*Math.sin(parent.denavit[3]));	matrix.m12=(float)(Math.cos(parent.denavit[2])*Math.sin(parent.denavit[3]));	matrix.m22=(float)Math.cos(parent.denavit[3]);
            matrix.m03=0;                                                    				matrix.m13=0;                                                    				matrix.m23=0;
            //Translation
            matrix.m30=(float)parent.denavit[0];
            matrix.m31=(float)(parent.denavit[1]*-Math.sin(parent.denavit[3]));
            matrix.m32=(float)(parent.denavit[1]*Math.cos(parent.denavit[3]));
            matrix.m33=1;
            return matrix;
        }
        public void getParametersFromMatrix(double parameters[],SItem parent) {
        	float matrix[] = new float[16];
            parent.aframe.matrix().get(matrix);

            parameters[0]=matrix[12];           //a
            if(-matrix[4] >0 && -matrix[0] >0)
            {
                parameters[2] = Math.acos(-matrix[4]) + PI_OVER_2;
            }else
            {
                if(matrix[4] >0 && -matrix[0] >0)
                {
                    parameters[2] = Math.acos(-matrix[4]) - 3*PI_OVER_2;
                }
                else
                {
                    parameters[2]=Math.asin(-matrix[4]);     //theta
                }
            }
            if(-matrix[9] >0 && -matrix[10] >0)
            {
                parameters[3] = Math.acos(-matrix[9]) + PI_OVER_2;
            }else
            {
                if(matrix[9] >0 && -matrix[10] >0)
                {
                    parameters[3] = Math.acos(-matrix[9]) - 3*PI_OVER_2;
                }
                else
                {
                    parameters[3]=Math.asin(-matrix[9]);     //alpha
                }
            }
            if(parameters[2] <= -3.14159)parameters[2]=3.14159;
            if(parameters[3] <= -3.14159)parameters[2]=3.14159;
            if(matrix[9]==0)
                parameters[1]=matrix[14];
            else
                if(matrix[10]==0)
                    parameters[1]=-matrix[13];
                    else
                        parameters[1]=matrix[14]/matrix[10];


            /*                                  CRAIG
            cos(theta),          -sin(theta),                    0,                    a(i-1)
            cos(alfa)*sin(theta), cos(alfa)*cos(theta),         -sin(alfa),         -sin(alfa)*d
            sin(alfa)*sin(theta), sin(alfa)*cos(theta),          cos(alfa),          cos(alfa)*d
            0,                    0,                             0,                    1
            */
        }

        public void Update(){
        	
        }
        public void EslabonMoving(int id){}

        public void setOriginalData(Vector<String> value){
        	// order [0]=a [1]=d [2]=theta [3] =alpha
        	
        }
        public void setq(double d){}

        public simunrobot.core.Modes Mode(){return mode;}

        public void setName(String Name){}
        public String getName(){return mName;}
        public void setChanged(){}
        public void setId(int id){_id=id;}
        public int getId(){return _id;}

        public void graficarmatrix(double matrix[],String titulo){}

        SItem parentItem;
        public InteractiveFrame aframe;

        double OriginalParameters[];
        geoObjectMini Geometria;

    
        double denavit[];  // order [0]=a [1]=d [2]=theta [3] =alpha
        double amin,amax,dmin,dmax,thetamin,thetamax,alphamin,alphamax;

        int _id;
        String mName;
        simunrobot.core.Modes mode;

        Vector<SItem> childItems;
        boolean changedbywidget;
        public Item type;
}
